#include "rendPlatform.hpp"
#include "rendVect.hpp"

#include <stdio.h>
#include <math.h>

static bool
compare(
	const rend::matx4& a,
	const rend::matx4& b,
	const float epsilon = 1e-6)
{
	for (unsigned i = 0; i < 4; ++i)
		for (unsigned j = 0; j < 4; ++j)
			if (fabsf(a[i][j] - b[i][j]) > epsilon)
				return false;

	return true;
}


int main(
	int argc,
	char** argv)
{
	int result = 0;

	const unsigned bits_per_dimension = 3;
	const unsigned mask_per_dimension = (1 << bits_per_dimension) - 1;
	const int bias_per_dimension = int(1 << bits_per_dimension) / 2;

	for (unsigned k = 0; k < (1 << bits_per_dimension * 3); ++k)
	{
		rend::vect3 axis(
			float(int((k >> bits_per_dimension * 2) & mask_per_dimension) - bias_per_dimension),
			float(int((k >> bits_per_dimension * 1) & mask_per_dimension) - bias_per_dimension),
			float(int((k >> bits_per_dimension * 0) & mask_per_dimension) - bias_per_dimension));

		if (axis.sqr() == 0.f)
			continue;

		axis.normalise();

		printf("axis: %f, %f, %f\n",
			axis[0],
			axis[1],
			axis[2]);

		const unsigned arc_steps = 16;

		for (unsigned i = 0; i < arc_steps; ++i)
		{
			const float angle = M_PI * 2.f * float(i) / float(arc_steps);

			printf("angle: %f\n", angle);

			const rend::quat q(angle, axis);

			const rend::matx4 mq(q);

			rend::matx4 mr;
			mr.rotate(
				angle,
				axis[0],
				axis[1],
				axis[2]);

			const bool same = compare(mq, mr);

			printf(
				"mq:\t%f, %f, %f\tmr:\t%f, %f, %f\t\t%s\n"
				"\t%f, %f, %f\t\t%f, %f, %f\n"
				"\t%f, %f, %f\t\t%f, %f, %f\n",
				mq[0][0], mq[0][1], mq[0][2],
				mr[0][0], mr[0][1], mr[0][2],
				same ? "same" : "differ",
				mq[1][0], mq[1][1], mq[1][2],
				mr[1][0], mr[1][1], mr[1][2],
				mq[2][0], mq[2][1], mq[2][2],
				mr[2][0], mr[2][1], mr[2][2]);

			if (!same)
				result = -1;
		}
	}

	rend::vect3 axis[2];
	axis[0].normalise(rend::vect3(0.f, 1.f, 1.f));
	axis[1].normalise(rend::vect3(1.f, 1.f, 0.f));

	const float angle[2] =
	{
		M_PI * .5f, 
		M_PI * .25f
	};

	rend::quat p(angle[1], axis[1]);
	rend::quat q(angle[0], axis[0]);
	q.qmul(p);

	const rend::matx4 mq(q);

	rend::matx4 mp;
	rend::matx4 mr;

	mp.rotate(angle[1], axis[1][0], axis[1][1], axis[1][2]);
	mr.rotate(angle[0], axis[0][0], axis[0][1], axis[0][2]);
	mr.mul(mp);

	const bool same = compare(mq, mr);

	printf(
		"multi\nmq:\t%f, %f, %f\tmr:\t%f, %f, %f\t\t%s\n"
		"\t%f, %f, %f\t\t%f, %f, %f\n"
		"\t%f, %f, %f\t\t%f, %f, %f\n",
		mq[0][0], mq[0][1], mq[0][2],
		mr[0][0], mr[0][1], mr[0][2],
		same ? "same" : "differ",
		mq[1][0], mq[1][1], mq[1][2],
		mr[1][0], mr[1][1], mr[1][2],
		mq[2][0], mq[2][1], mq[2][2],
		mr[2][0], mr[2][1], mr[2][2]);

	if (!same)
		result = -1;

	return result;
}

